#include <motor_RS485.hpp>
#include <motor_CAN.hpp>
#include <chrono>
#include <iostream>
#include <unistd.h>

using namespace std;

int main()
{
    cout << "Single Motor Test Start" << endl;
    Motor_RS485 motor;
    MotorConfig cfg;
    cfg.address = 1;
    cfg.poleCount = 15;
    if (!motor.InitBySerialDevice("/dev/ttyUSB0", 115200, cfg, false))
    {
        cout << "motor init fail" << endl;
        return -1;
    }
    float rpm = motor.GetRPM();
    // motor.SetControlMode(Motor::ControlMode::Speed);
    motor.SetControlMode(ControlMode::Duty);

    while (true)
    {
        // auto start = chrono::steady_clock::now();
        motor.SendHeartBeat();
        // cout << "us" << chrono::duration_cast<chrono::microseconds>(chrono::steady_clock::now() - start).count()
        //      << endl;
        float rpm = motor.GetRPM();
        motor.SetPWM(0);
        cout << "rpm:" << rpm << endl;
    }

    return 0;
}
